#include "chassis.h"
#include <stdlib.h>

Chassis::Chassis()
{
    force_velocity_conversion_coefficient = linear_velocity_max / (force_limit - force_threshold);
    torque_velocity_conversion_coefficient = angular_velocity_max / (torque_limit - torque_threshold);
}

int Chassis::ChassisVelocity2WheelVelocity(const ChassisVelocity &chassis_velocity, WheelVelocity &wheel_velocity)
{
//    wheel_velocity.left = chassis_velocity.linear - chassis_velocity.angular / 2;
//    wheel_velocity.right = chassis_velocity.linear + chassis_velocity.angular / 2;
    wheel_velocity.left = ( 2 * chassis_velocity.linear - chassis_velocity.angular * l ) / 2;
    wheel_velocity.right = ( 2 * chassis_velocity.linear + chassis_velocity.angular * l ) / 2;

    wheel_velocity.left = wheel_velocity.left / M_PI / wheel_diameter * 60 * reduction_ratio;
    wheel_velocity.right = wheel_velocity.right / M_PI / wheel_diameter * 60 * reduction_ratio;
    return 0;
}

int Chassis::ChassisMoventForceGuide(ChassisForceSensorValue &chassis_force_sensor_value, WheelVelocity &wheel_velocity)
{
    ChassisVelocity chassis_velocity;

    if(chassis_force_sensor_value.force > 1.0 * force_limit)
    {
        chassis_force_sensor_value.force = force_limit;
    }
    else if(chassis_force_sensor_value.force < -1.0 * force_limit)
    {
        chassis_force_sensor_value.force = -force_limit;
    }

    if(chassis_force_sensor_value.torque > 1.0 * torque_limit)
    {
        chassis_force_sensor_value.torque = torque_limit;
    }
    else if(chassis_force_sensor_value.torque < -1.0 * torque_limit)
    {
        chassis_force_sensor_value.torque = -torque_limit;
    }

    // linear velocity
    if (abs(chassis_force_sensor_value.force) < force_threshold)
    {
        chassis_velocity.linear = 0.0;
    }
    else if (abs(chassis_force_sensor_value.force) >= force_threshold && abs(chassis_force_sensor_value.force) <= force_limit)
    {
        if(chassis_force_sensor_value.force < 0)
            chassis_velocity.linear = force_velocity_conversion_coefficient * (chassis_force_sensor_value.force + force_threshold);
        else
            chassis_velocity.linear = force_velocity_conversion_coefficient * (chassis_force_sensor_value.force - force_threshold);
    }

    // angular velocity
    if (abs(chassis_force_sensor_value.torque) < torque_threshold)
    {
        chassis_velocity.angular = 0.0;
    }
    else if (abs(chassis_force_sensor_value.torque) >= torque_threshold && abs(chassis_force_sensor_value.torque) <= torque_limit)
    {
        if(chassis_force_sensor_value.torque < 0)
            chassis_velocity.angular = torque_velocity_conversion_coefficient * (chassis_force_sensor_value.torque + torque_threshold);
        else
            chassis_velocity.angular = torque_velocity_conversion_coefficient * (chassis_force_sensor_value.torque - torque_threshold);
    }

    // chassis velocity to wheel velocity
//    chassis_velocity.angular = 0;
    ChassisVelocity2WheelVelocity(chassis_velocity, wheel_velocity);

    return 0;
}
